#
# Context Library
#   Context class
#   Generating Algorithms
#
# This is that standard place to put algorithms that will create a context -
#   estimate the location of the player and the objects around him.
#

import math
import robomath

class Context:
    def __init__(self):
        self.dynamic = []        # list of dynamic elements -
                                 #  (type, x, y, dx, dy, [team], [unum])
                                 #  type - [ 'p' | 'b' | 'o' ] - player, ball, opponent
    
        self.x = None            # location
        self.y = None

        self.dx = None           # velocity
        self.dy = None

        self.direction = 0       # direction
    
    def update_self(self, time, new_location, new_direction, new_velocity, certainty=1):
        """Input: a tuple - (x,y) and a float dir, which just happens to be the output 
        of the robomath.triangulate and robomath.find_angle functions
        """
        if new_location != None and new_direction != None and new_velocity != None:
            (self.x, self.y), (self.dx, self.dy) = new_location, new_velocity
            self.direction = new_direction*certainty + self.direction*(1-certainty)
            
        # if new_velocity != (None, None): print time, new_velocity
        # print "Params:", (self.x, self.y), (self.dx, self.dy), math.degrees(self.direction)    
    
    def update_dynamic(self):
        pass
    
    def update_sight(self, WM):
        """This method is run every time we recieve new sight information"""
        certainty = 0.5
        
        # take all info from objects that are static
        objs = [ (robomath.locate(x[0], WM.side) + tuple(map(float, x[1:]))) \
            for x in WM.current.vision[2:] if x[0][0] in ['f', 'g'] ]
        
        # sort objects by distance
        objs.sort(cmp=(lambda x,y: int(x[2]-y[2])))
        
        #print WM.current.vision[2:]
        #print [x for x in objs if len(x) > 4]
        
        # location is best found by the nearest four
        location = robomath.calculate_location(objs[:4])
        
        if location == None:
            direction = None
            velocity = None
        else:
            direction = robomath.calculate_direction(location, objs[:4])
            velocity = robomath.calculate_velocity(objs=[x for x in objs if len(x) > 4], loc=location)        
        
        # update our location
        self.update_self(WM.current.vision[1], location, direction, velocity, certainty)        
    
    def update(self, WM):
        pass
